import java.text.ParseException;
import java.util.List;
import org.armedbear.lisp.ControlTransfer;
import jade.semantics.lang.sl.grammar.Term;
import iRobotCreate.Controller;
import iRobotCreate.IRobotState;
import iRobotCreate.StateBasedController;
import iRobotCreate.iRobotCommands;
import iRobotCreate.iRobotCommands.Sensor;
import iRobotCreate.simulator.Environment;
import casa.LispAccessible;
import casa.ML;
import casa.MLMessage;
import casa.PerformDescriptor;
import casa.Status;
import casa.TransientAgent;
import casa.URLDescriptor;
import casa.abcl.CasaLispOperator;
import casa.abcl.ParamsMap;
import casa.conversation2.SubscribeClientConversation;
import casa.event.TimeEvent;
import casa.exceptions.URLDescriptorException;
import casa.ui.AgentUI;
import casa.util.CASAUtil;

public class AttackerAgent extends StateBasedController{
	public String ROBOT_BAM = "8";
	public String myPort = "9108";
	//public String ROBOT_AGENT = "136.159.7.26:9108";
	public String ROBOT_AGENT = "9100";
	public String CONTROLLER_LAC = "136.159.7.26:9000";
	//public String CAMERA_AGENT = "136.159.7.26:8995";
	public String CAMERA_AGENT = "8995";
	public String COLOR = "purple";	
	public boolean GOALZERO = true;
	public String MYCOLOR = null;
	public String OPCOLOR = null;
	public String PARTNERCOLOR = null;
	public String OP2COLOR = null;
	public double calcAngle = 0;
	public String[] thecoord = null;
	public int ballPos_x = 0, ballPos_y = 0, roboPos_x = 0, roboPos_y = 0, roboAngle = 0, goalPos_x = 639, goalPos_y = 0, offset_x = 0, offset_y = 0;
	public double SCALE = 2;
	public boolean defenseLoop= true;
	
	//public int SCALE = 2304/1280;
	//public int SCALE = 1;
	
	static {
		  createCasaLispOperators(AttackerAgent.class);
	  }
	
	public AttackerAgent(ParamsMap params, AgentUI ui) throws Exception {
		super(params, ui);
	}
	
	@Override
	public void initializeAfterRegistered(boolean registered){
		super.initializeAfterRegistered(registered);
			
		try{
			@SuppressWarnings("unused")
			SubscribeClientConversation convBump = new SubscribeClientConversation(
					"--subscription-request", 
					this, getServer(), 
					"(all ?x (BumpsAndWheelDrops ?x))", null)
			{
				@Override
				public void update(URLDescriptor agentB, Term term) {
					if (term==null || demoOn())
						return;
					String intString = term.toString();
					int val = Integer.parseInt(intString);
					onBumpsAndWheelDrops(val);
				}
			};
			
			/*
			@SuppressWarnings("unused")
			SubscribeClientConversation convOverCurr = new SubscribeClientConversation(
					"--subscription-request", 
					this, getServer(), 
					"(all ?x (Overcurrents ?x))", null)
			{
				@Override
				public void update(URLDescriptor agentB, Term term) {
					if (term==null || demoOn())
						return;
					String intString = term.toString();
					int val = Integer.parseInt(intString);
					onOvercurrent(val);
				}
			};
			*/	
		}
		catch (Throwable e)
		{
			println("error", "Controller.initializeAfterRegistered().SubscribeConversation", e);
		}
		//Startup robot if necessary. Wag to indicate startup.
		try {
			sendMessage(ML.REQUEST, ML.EXECUTE, new URLDescriptor(CONTROLLER_LAC), ML.CONTENT, "(start-robot "+ROBOT_BAM+")");
			//sendMessage(ML.REQUEST, ML.EXECUTE, new URLDescriptor(ROBOT_AGENT), ML.CONTENT, "(progn () (irobot.rotate-deg -30) (irobot.rotate-deg -30) )");
			sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
					ML.LANGUAGE, "lisp",
					ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.rotate-deg -30) (irobot.rotate-deg 30))");
			//sendMessage(ML.REQUEST, ML.EXECUTE, new URLDescriptor(ROBOT_AGENT), ML.CONTENT, "(irobot.rotate-deg 30)");
		} catch (URLDescriptorException e) {
			e.printStackTrace();
		}
		AgentUI thatAgent = this.getUI();		
		thatAgent.println("Robit initialized.");
		setState(waiting);
	}	
	
	//Template from Prof. Kremer's slides
	private CasaLispOperator SOCCER_START = new CasaLispOperator("start", "\"!Start robots\" "
			+"MY \"@java.lang.String\" \"Define the robot by colour\" "
			+"&KEY "
			+"&OPTIONAL PARTNER \"@java.lang.String\" \"Define partner's colour\" "
			+"&OPTIONAL OP1 \"@java.lang.String\" \"Define opponent 1's colour\" "
			+"&OPTIONAL OP2 \"@java.lang.String\" \"Define opponent 2's colour\" "
			+"&OPTIONAL (GOALZERO NIL) \"@boolean\" \"Declares status of scoring\" "
			, AttackerAgent.class, "Roberto") {		
		@Override
		public Status execute(TransientAgent agent, ParamsMap params, AgentUI ui,
				org.armedbear.lisp.Environment env) throws ControlTransfer {
			if (params.containsKey("MY")) {
				if (params.containsKey("OP2") && params.containsKey("PARTNER"))	{
					MYCOLOR = params.getJavaObject("MY", String.class);
					PARTNERCOLOR = params.getJavaObject("PARTNER", String.class);
					OPCOLOR = params.getJavaObject("OP", String.class);
					OP2COLOR = params.getJavaObject("OP2", String.class);
					GOALZERO = params.getJavaObject("GOALZERO", Boolean.class);
					if (GOALZERO)
						goalPos_y = 0;
					else
						goalPos_y = 700;
				}
				else	{
					COLOR = params.getJavaObject("MY", String.class);
					OPCOLOR = params.getJavaObject("OP", String.class);
					GOALZERO = params.getJavaObject("GOALZERO", Boolean.class);
				}
				setState(calculate);
			}
			else	{
				return new Status(
				Status.UNKNOWN_ERROR,
				"Enter command as (start :my <color> :partner <color> :op1 <color> :op2 <color> :goalzero <bool>) with at least the color for 'my'");
			}    	  
			return null;
		}
	};
	
	
	public void DefenseMode(){
		if (OPCOLOR != null)
		{
			
				MLMessage rcoord = findObject("triangle", OPCOLOR); 
				thecoord = rcoord.getParameter(ML.CONTENT).split(",");
				roboPos_x = Integer.parseInt(thecoord[1]);
				roboPos_y = Integer.parseInt(thecoord[2]);
				roboAngle = Integer.parseInt(thecoord[3]);
				
				int drivebyY = goalPos_y - roboPos_y;
				int drivebyX = goalPos_x - roboPos_y;
				
				int tempAngle;
				
				if (drivebyY > 0)
				{
					sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
							ML.LANGUAGE, "lisp",
							ML.CONTENT, "(progn () (irobot.rotate-deg -"+roboAngle+") (irobot.rotate-deg -90) (irobot.moveby "+drivebyY+"))");
				}
				else{
					drivebyY = -drivebyY;
					sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
							ML.LANGUAGE, "lisp",
							ML.CONTENT, "(progn () (irobot.rotate-deg -"+roboAngle+") (irobot.rotate-deg 90) (irobot.moveby "+drivebyY+"))");
				}
				
				
				if (drivebyX > 0){
					drivebyX = -drivebyX;
					sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
							ML.LANGUAGE, "lisp",
							ML.CONTENT, "(progn () (irobot.rotate-deg -"+roboAngle+") (irobot.moveby "+drivebyX+"))");
				}
				else
				{
					sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
							ML.LANGUAGE, "lisp",
							ML.CONTENT, "(progn () (irobot.rotate-deg -"+roboAngle+") ((irobot.rotate-deg 180) (irobot.moveby "+drivebyX+"))");
				}
				
				
			while(defenseLoop)
			{
				sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
						ML.LANGUAGE, "lisp",
						ML.CONTENT, "(progn () (irobot.moveby 220) (irobot.rotate-deg 180) (irobot.moveby 220))");
			}
		}
	}
	
	//Find angle
	// x' = x(cos(theta)) - y(sin(theta))
	// y' = x(sin(theta)) + y(cos(theta))
	
	//public static double atan(double a)
	//public static double toDegrees(double angrad)
	
	@Override
	protected void onBumpsAndWheelDrops(int val){
		getCurrentState().handleEvent(Sensor.BumpsAndWheelDrops, (short)val);
	}
	//Hits a wall or another robot
	protected void onOvercurrent(int val){
		getCurrentState().handleEvent(Sensor.Overcurrents, (short)val);
	}
	
	//WAITING STATE
	IRobotState waiting = new IRobotState("waiting"){		
		@Override
		public void enterState(){
			System.out.println("Waiting State");
			sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
					ML.CONTENT, "(progn () (irobot.drive 0 :flush T))");
		}
		
		@Override
		public void handleEvent(Sensor sensor, short reading) {			
		}		
	};
	
	//CALCULATE STATE
	IRobotState calculate = new IRobotState("calculate"){
		@Override
		public void enterState(){
			System.out.println("Calculate State");
			makeSubthread (new Runnable() {
				@Override public void run() {
					//Find ball
					try {
						MLMessage bcoord = findObject("circle", "red"); 
						thecoord = bcoord.getParameter(ML.CONTENT).split(",");
						ballPos_x = Integer.parseInt(thecoord[1]);
						ballPos_y = Integer.parseInt(thecoord[2]);

						//Find my sexy robot self
						MLMessage rcoord = findObject("triangle", COLOR); 
						thecoord = rcoord.getParameter(ML.CONTENT).split(",");
						roboPos_x = Integer.parseInt(thecoord[1]);
						roboPos_y = Integer.parseInt(thecoord[2]);
						roboAngle = Integer.parseInt(thecoord[3]);
						
						if (ballPos_x <= 0 || ballPos_x > 1200 || ballPos_y <= 0 || ballPos_y > 700 || roboAngle <= 0)
							setState(lost);
						
						if (GOALZERO) {
							if (ballPos_y < 50 && ballPos_x > 425 && ballPos_x < 852)
								setState(win);
						}else {
							if (ballPos_y > 650 && ballPos_x > 425 && ballPos_x < 852)
								setState(win);
						}
						
						//Find ball and offset to go to in order to push ball into goal and attain certain victory
						double opposite = Math.abs(goalPos_y-ballPos_y);
						double adjacent = Math.abs(goalPos_x-ballPos_x);
						double hypotenuse = (Math.sqrt(((opposite*opposite))+(adjacent*adjacent)));
						double slope = Math.abs((opposite)/(adjacent));
						double goalAngle = Math.atan(slope)*180/Math.PI;
						//offset_x = (int) (Math.cos(goalAngle)*iRobotCommands.chassisRadius*SCALE);
						//offset_y = (int) (Math.sin(goalAngle)*iRobotCommands.chassisRadius*SCALE);
						double offset_scale = iRobotCommands.chassisRadius/hypotenuse;
						//Set offset
						if (GOALZERO) {
							if (ballPos_x <= goalPos_x) {
								offset_x = (int) (ballPos_x - adjacent*offset_scale);
								offset_y = (int) (ballPos_y + opposite*offset_scale);
							}else {
								offset_x = (int) (ballPos_x + adjacent*offset_scale);
								offset_y = (int) (ballPos_y + opposite*offset_scale);
							}
						}else {
							if (ballPos_x <= goalPos_x) {
								offset_x = (int) (ballPos_x - adjacent*offset_scale);
								offset_y = (int) (ballPos_y - opposite*offset_scale);
							}else {
								offset_x = (int) (ballPos_x + adjacent*offset_scale);
								offset_y = (int) (ballPos_y - opposite*offset_scale);
							}
						}						
						System.out.println("Offset x = "+offset_x+" and Offset y = "+offset_y);
						if (offset_x < 0)
							offset_x = 0;
						if (offset_y < 0)
							offset_y = 0;
						if (offset_x > 1280)
							offset_x = 1280;
						if (offset_y > 700)
							offset_y = 700;
						
						//Find how to get to offset
						double offset_opposite = Math.abs(roboPos_y-offset_y);
						double offset_adjacent = Math.abs(roboPos_x-offset_x);
						double offset_slope = Math.abs((offset_opposite)/(offset_adjacent));
						double offsetAngle = Math.atan(offset_slope)*180/Math.PI;
						double finishingAngle = offsetAngle;
						if(offset_y <= roboPos_y && offset_x <= roboPos_x) {
							finishingAngle = 180 + offsetAngle;
							offsetAngle = 180 - roboAngle + offsetAngle;							
							System.out.println("offset is left and above");
						}
						else if(offset_y <= roboPos_y && offset_x >= roboPos_x){
							finishingAngle = 360 - offsetAngle;
							offsetAngle = 360 - roboAngle - offsetAngle;							
							System.out.println("Offset is right and above");
						}
						else if(offset_y >= roboPos_y && offset_x <= roboPos_x){					
							finishingAngle = 180 - offsetAngle;
							offsetAngle = 180 - roboAngle - offsetAngle;							
							System.out.println("Offset is left and below");
						}
						else {
							finishingAngle = offsetAngle;
							offsetAngle = 360 - roboAngle + offsetAngle;							
							System.out.println("Offset is right and below");
						}						
						
						//Find angle to turn after getting to offset
						if (GOALZERO) {
							if (ballPos_x <= goalPos_x) {
								finishingAngle = 360 - finishingAngle - goalAngle;
							}else {
								finishingAngle = 180 - finishingAngle + goalAngle;
							}
						}else {
							if (ballPos_x <= goalPos_x) {
								finishingAngle = 360 - finishingAngle + goalAngle;	
							}else {
								finishingAngle = 180 - finishingAngle - goalAngle;
							}
						}						

						// Negate directions for correct turn directions and make integer
						int turnAngle = (int) -offsetAngle;
						int finishingTurnAngle = (int) -finishingAngle;
						
						//Find distance to offset
						int driveBy = (int) (Math.sqrt(((offset_opposite*offset_opposite))+(offset_adjacent*offset_adjacent))*SCALE);
						System.out.println("Driving by "+String.valueOf(driveBy));
						//Turn toward luscious curvy ball of pleasure and head there
						sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
								ML.LANGUAGE, "lisp",
								ML.CONTENT, "(progn () (irobot.rotate-deg "+turnAngle+") (irobot.moveby "+driveBy+") (irobot.rotate-deg "+finishingTurnAngle+") (irobot.drive 100))");
						/*
						double opposite = Math.abs(roboPos_y-ballPos_y);
						double adjacent = Math.abs(ballPos_x-roboPos_x);
						double slope = Math.abs((opposite)/(adjacent));
						double tempAngle = Math.atan(slope)*180/Math.PI;
						//System.out.println(String.valueOf(tempAngle));
						if(ballPos_y <= roboPos_y && ballPos_x <= roboPos_x) {
							//tempAngle = 180 + tempAngle;
							tempAngle = 180 - roboAngle + tempAngle;
							System.out.println("Ball is left and above");
						}
						else if(ballPos_y <= roboPos_y && ballPos_x >= roboPos_x){
							//tempAngle = 360 - tempAngle;
							tempAngle = 360 - roboAngle - tempAngle;
							System.out.println("Ball is right and above");
						}
						else if(ballPos_y >= roboPos_y && ballPos_x <= roboPos_x){					
							//tempAngle = 180 - tempAngle;
							tempAngle = 180 - roboAngle - tempAngle;
							System.out.println("Ball is left and below");
						}
						else {
							tempAngle = 360 - roboAngle + tempAngle;
							System.out.println("Ball is right and below");
						}						
						
						// Negate direction for correct turn direction
						int turnAngle = (int) -tempAngle;
						//Turn toward luscious curvy ball of pleasure
						sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
								ML.LANGUAGE, "lisp",
								ML.CONTENT, "(progn () (irobot.rotate-deg "+turnAngle+"))");
						*/
						
						}
						catch(Throwable e) {
							e.printStackTrace();
							System.out.println("ERROR");
						}
						report();
						CASAUtil.sleepIgnoringInterrupts(1000, null);
						try {
							sleep(1000);
						} catch (InterruptedException e) {
							e.printStackTrace();
						}
						setState(pushingBall);
					}
				}).start() ;			
		}
		
		@Override 
		public void handleEvent(Sensor sensor, short reading){
			switch(sensor){
			case Overcurrents:
				sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
						ML.LANGUAGE, "lisp",
						ML.CONTENT, "(irobot.drive 0 :flush T)");
				setState(calculate);
				break;
			default:
				break;
			}
		}
	};
	
	//MOVINGTOBALL STATE
	IRobotState movingToBall = new IRobotState("movingToBall"){
		@Override
		public void enterState(){
			System.out.println("MovingToBall State");
			makeSubthread (new Runnable() {
				@Override public void run() {
					//Find ball
					try {
						MLMessage bcoord = findObject("circle", "red"); 
						thecoord = bcoord.getParameter(ML.CONTENT).split(",");
						if (Integer.parseInt(thecoord[1]) < ballPos_x - 25 || Integer.parseInt(thecoord[1]) > ballPos_x + 25 || Integer.parseInt(thecoord[2]) < ballPos_y - 25 || Integer.parseInt(thecoord[2]) > ballPos_y + 25)
							setState(calculate);
						else {
							ballPos_x = Integer.parseInt(thecoord[1]);
							ballPos_y = Integer.parseInt(thecoord[2]);
							
							MLMessage rcoord = findObject("triangle", COLOR); 
							thecoord = rcoord.getParameter(ML.CONTENT).split(",");
							roboPos_x = Integer.parseInt(thecoord[1]);
							roboPos_y = Integer.parseInt(thecoord[2]);
							roboAngle = Integer.parseInt(thecoord[3]);
							
							if (roboPos_x < offset_x - 35 || roboPos_x > offset_x + 35 || roboPos_y < offset_y - 35 || roboPos_y > offset_y + 35)
								setState(calculate);												// If not close enough to offset, recalculate
							
							double opposite = Math.abs(roboPos_y-ballPos_y);
							double adjacent = Math.abs(ballPos_x-roboPos_x);
							double slope = Math.abs((opposite)/(adjacent));
							double tempAngle = Math.atan(slope)*180/Math.PI;
							if(ballPos_y <= roboPos_y && ballPos_x <= roboPos_x) {
								tempAngle = 180 - roboAngle + tempAngle;
								System.out.println("Ball is left and above");
							}
							else if(ballPos_y <= roboPos_y && ballPos_x >= roboPos_x){
								tempAngle = 360 - roboAngle - tempAngle;
								System.out.println("Ball is right and above");
							}
							else if(ballPos_y >= roboPos_y && ballPos_x <= roboPos_x){					
								tempAngle = 180 - roboAngle - tempAngle;
								System.out.println("Ball is left and below");
							}
							else {
								tempAngle = 360 - roboAngle + tempAngle;
								System.out.println("Ball is right and below");
							}						
							
							// Negate direction for correct turn direction
							int turnAngle = (int) -tempAngle;
							//Turn toward luscious curvy ball of pleasure
							sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
									ML.LANGUAGE, "lisp",
									ML.CONTENT, "(progn () (irobot.rotate-deg "+turnAngle+") (irobot.drive 100))");						
							
							//setState(pushingBall);
						}
					}
					catch(Throwable e) {
						e.printStackTrace();
						System.out.println("ERROR");
					}
				}
			}).start() ;

			CASAUtil.sleepIgnoringInterrupts(1000, null);
			//setState(calculate);
		}
		
		@Override
		public void handleEvent(Sensor sensor, short reading) {
			switch(sensor) {
			case Overcurrents:
				sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
						ML.LANGUAGE, "lisp",
						ML.CONTENT, "(irobot.drive 0 :flush T)");
				setState(calculate);
			default:
				break;	
			}			
		}
		
	};

	//PUSHINGBALL STATE
	IRobotState pushingBall = new IRobotState("pushingBall"){
		@Override
		public void enterState(){
			System.out.println("PushingBall State");
			sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
					ML.LANGUAGE, "lisp",
					ML.CONTENT, "(progn () (irobot.drive 100))");
		}
		
		@Override
		public void handleEvent(Sensor sensor, short reading) {
			switch(sensor) {
			case Overcurrents:
				sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
						ML.LANGUAGE, "lisp",
						ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.moveby -200))");
				setState(calculate);
			default:
				break;
			}
		}		
	};
	
	//LOST STATE
	IRobotState lost = new IRobotState("lost"){
		@Override
		public void enterState(){
			System.out.println("Lost...");
			setState(calculate);
		}
		
		@Override
		public void handleEvent(Sensor sensor, short reading) {
			
		}		
	};

	public MLMessage findObject (String shape, String colour) {
		MLMessage coord = null;
		try {
			//System.out.println("Attempting to find object...");
			coord = sendRequestAndWait(ML.REQUEST, "get-color-position", new URLDescriptor(CAMERA_AGENT), ML.CONTENT, shape+","+colour);
		} catch (URLDescriptorException e) {
			e.printStackTrace();
			System.out.println("Failed to find object :(");
		}	
		return coord;
	}
	
	//WIN STATE
	IRobotState win = new IRobotState("win"){
		@Override
		public void enterState(){
			sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
					ML.LANGUAGE, "lisp",
					ML.CONTENT, "(progn () (irobot.drive 0 :flush T) (irobot.rotate-deg 360))");
		}
		@Override
		public void handleEvent(Sensor sensor, short reading) {
			// TODO Auto-generated method stub
			
		}
	};
		
	//Report current state and position
	@LispAccessible(
			name = "report"
			)
	public void report(){
		AgentUI thatAgent = this.getUI();		
		thatAgent.println("My current state is: "+getCurrentStateName());
		thatAgent.println("Sexy robot self: x = "+roboPos_x+" y = "+roboPos_y+" angle = "+roboAngle);
		thatAgent.println("Curvaceous ball: x = "+ballPos_x+" y = "+ballPos_y);
	}
	
	//Stop iRobot from whatever it is doing
	@LispAccessible(
			name = "reset"
			)
	public void reset(){
		sendMessage(ML.REQUEST, ML.EXECUTE, getServer(),
				ML.LANGUAGE, "lisp",
				ML.CONTENT, "(irobot.drive 0 :flush T)");
		setState(waiting);
	}
	
	//Start iRobot with default values
	@LispAccessible(
			name = "staato"
			)
	public void staato() {
		setState(calculate);
	}
}